from src.Class import *
import numpy as np

def dian2(p1,p2,width,height):#第二种即两个中心点z不一样
    u=0
    p3=np.zeros(3)
    p4=np.zeros(3)
    p5=np.zeros(3)
    p6=np.zeros(3)
    p_1=np.zeros(3)
    p_2=np.zeros(3)
    p_3=np.zeros(3)
    p_4=np.zeros(3)
    p_5=np.zeros(3)
    p_6=np.zeros(3)
    p_7=np.zeros(3)
    p_8=np.zeros(3)
    if p1[1]-p2[1]==0:
        u=1
    if u==0:
        k=-(p1[0]-p2[0])/(p1[1]-p2[1])
        x=width/(2*(1+k**2)**0.5)
        p3[0]=p1[0]+x
        p3[1]=p1[1]+k*x
        p3[2]=p1[2]
        p4[0]=p1[0]-x
        p4[1]=p1[1]-k*x
        p4[2]=p1[2]
        p5[0]=p2[0]+x
        p5[1]=p2[1]+k*x
        p5[2]=p2[2]
        p6[0]=p2[0]-x
        p6[1]=p2[1]-k*x
        p6[2]=p2[2]
    else :
        p3[0]=p1[0]
        p3[1]=p1[1]+width/2
        p3[2]=p1[2]
        p4[0]=p1[0]
        p4[1]=p1[1]-width/2
        p4[2]=p1[2]
        p5[0]=p2[0]
        p5[1]=p2[1]+width/2
        p5[2]=p2[2]
        p6[0]=p2[0]
        p6[1]=p2[1]-width/2
        p6[2]=p2[2]
    a=(p2[1]-p1[1])*(p3[2]-p1[2])-(p3[1]-p1[1])*(p2[2]-p1[2])
    b=(p2[2]-p1[2])*(p3[0]-p1[0])-(p3[2]-p1[2])*(p2[0]-p1[0])
    c=(p2[0]-p1[0])*(p3[1]-p1[1])-(p3[0]-p1[0])*(p2[1]-p1[1])
    t=height/(2*(a**2+b**2+c**2)**0.5)
    p_3[0]=p3[0]+a*t
    p_3[1]=p3[1]+b*t
    p_3[2]=p3[2]+c*t
    p_2[0]=p3[0]-a*t
    p_2[1]=p3[1]-b*t
    p_2[2]=p3[2]-c*t
    p_4[0]=p4[0]+a*t
    p_4[1]=p4[1]+b*t
    p_4[2]=p4[2]+c*t
    p_1[0]=p4[0]-a*t
    p_1[1]=p4[1]-b*t
    p_1[2]=p4[2]-c*t
    p_7[0]=p5[0]+a*t
    p_7[1]=p5[1]+b*t
    p_7[2]=p5[2]+c*t
    p_6[0]=p5[0]-a*t
    p_6[1]=p5[1]-b*t
    p_6[2]=p5[2]-c*t
    p_8[0]=p6[0]+a*t
    p_8[1]=p6[1]+b*t
    p_8[2]=p6[2]+c*t
    p_5[0]=p6[0]-a*t
    p_5[1]=p6[1]-b*t
    p_5[2]=p6[2]-c*t
    list = [p_3,p_7,p_6,p_2,p_4,p_8,p_5,p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new

def pipes_to_region(p,bs):
    import numpy as np
    import copy
    a = []
    b = []
    c = []
    p4 = []
    pipes = []
    blocks = []
    e1 = []
    e2 = []
    e3 = []
    area = []
    area1 = []
    area2 = []
    area3 = []
    block = []
    block1 = []
    block2 = []
    temp = 1000
    for i in range(len(p)):
        p1 = p[i]
        for j in range(len(p1)):
            p2 = p1[j]
            p3 = dian2(p2[0],p2[1],p2[2],p2[3])
            p4.append(p3)
    pipes = [p4[i:i + len(p1)] for i in range(0, len(p))]
    for i in range(len(bs)):
        b1 = bs[i]
        b2 = dian2(b1[0],b1[1],b1[2],b1[3])
        blocks.append(b2)
    for w in range(len(pipes)):
        a = pipes[w]
        b=np.array(a)
        d=np.array(b.reshape(int(b.size/3),3))
        e = d.tolist()
        for i in range(len(e)):
            e1.append(e[i][0])
            e2.append(e[i][1])
            e3.append(e[i][2])
        g1 = [min(e1),min(e2),min(e3)]
        g2 = [max(e1),max(e2),max(e3)]
        c.append([g1,g2])
        if c[0][0][0] > temp and c[0][0][1] > temp and c[0][0][2 ]> temp:
            area.append([[c[0][0][0]-temp,c[0][0][1]-temp,c[0][0][2]-temp],[c[0][1][0]+temp,c[0][1][1]+temp,c[0][1][2]+temp]])
        else:
             area.append([c[0][0],[c[0][1][0] + temp, c[0][1][1] + temp, c[0][1][2] + temp]])
        # area3 = [area[0][0],[(area[0][0][0]+area[0][1][0])/2,(area[0][0][1]+area[0][1][1])/2,(area[0][0][2]+area[0][1][2])/2],area[0][1]]
        # area4 = point_3_to_8(area3)
        # area2.append(area4)
        c.clear()
        e1.clear()
        e2.clear()
        e3.clear()
        for v in range(len(blocks)):
            f = copy.deepcopy(blocks[v])
            f.append([(f[0][0]+f[1][0])/2,(f[0][1]+f[1][1])/2,(f[0][2]+f[2][2])/2])
            i = 0
            while i<=8:
                if area[0][0][0] <= f[i][0] <= area[0][1][0] and area[0][0][1] <= f[i][1] <= area[0][1][1] and area[0][0][2] <= f[i][2] <= area[0][1][2]:
                    if len(area1) == 0:
                        area1.append(bs[v])
                        # print(area1)
                i+=1
            block.extend(area1)
            f.clear()
            block1 = copy.deepcopy(block)
            area1.clear()
        block.clear()
        block2.append(block1)
        area.clear()
    # print("所有管道区域分别绑定的障碍物为",block2)
    # print(len(block2))
    # print("所有管道的区域为",area2)
    return block2

